/********************************************************************/
/*          MultiBoot.c                                             */
/*            AGB Multi-play Boot Library                           */
/*            (Multi-play Boot Server Support Library)              */
/*                                                                  */
/*          Copyright (C) 1999-2001 NINTENDO Co.,Ltd.               */
/********************************************************************/

#include "MultiBoot.h"


static u16 MultiBoot_required_data[MULTIBOOT_NCHILD];


/*------------------------------------------------------------------*/
/*                   Multi-play Boot Initialization                 */
/*------------------------------------------------------------------*/

void MultiBootInit(MultiBootParam *mp)
{
  mp -> client_bit = 0;
  mp -> probe_count = 0;
  mp -> response_bit = 0;

  mp -> check_wait = MULTIBOOT_CONNECTION_CHECK_WAIT;
  mp -> sendflag = 0;

  mp -> handshake_timeout = 0;

  *(vu16 *)REG_RCNT = 0;
  /* No Interrupt, 115k */
  *(vu16 *)REG_SIOCNT = SIO_MULTI_MODE | SIO_115200_BPS;
  *(vu16 *)REG_SIODATA8 = 0;
}


/*------------------------------------------------------------------*/
/*                   Multi-play Boot Main                           */
/*------------------------------------------------------------------*/
static int MultiBootSend(MultiBootParam *mp, u16 data);
static void MultiBootWaitSendDone(void);
static int MultiBootHandShake(MultiBootParam *mp);

int MultiBootMain(MultiBootParam *mp)
{
  extern void MultiBootStartProbe(MultiBootParam *mp);
  int i;
  int j;
  int k;

  if (MultiBootCheckComplete(mp)) {
    /* Transfer is complete so no other processing done */
    return 0;
  }

  if (mp -> check_wait > MULTIBOOT_CONNECTION_CHECK_WAIT) {
    /* After system call error, do not send anything,
     * and wait for client to have timeout error.
     */
    mp -> check_wait--;
    return 0;
  }

output_burst:
  if (mp -> sendflag) {
    mp -> sendflag = 0;
    /* There is a problem if SC7,SC6 is on at this point.
     * There may be a problem with the connection(connected to JOY) and      
     * communication does not have timeout error.
     * When the connection ID is not 00 or there is a problem with the SD or SI terminals an error occurs.
     */
    i = *(vu16 *)REG_SIOCNT & (SIO_MULTI_BUSY | SIO_ERROR | SIO_ID_NO_MASK | SIO_MULTI_SD | SIO_MULTI_SI);
    if (i != SIO_MULTI_SD) {
      MultiBootInit(mp);
      return i ^ SIO_MULTI_SD;
    }
  }

  if (mp -> probe_count >= 0xe0) {
    /* Check(handshake) to see if all slaves booted properly. */
    i = MultiBootHandShake(mp);
    if (i) {
      return i;
    }
    /* If Low speed recognition mode, handshake also 2bytes communication,call
     * If High speed recognition mode, handshake also high speed communication.
     */
    if (mp -> server_type == MULTIBOOT_SERVER_TYPE_QUICK
        && mp -> probe_count > 0xe1
        && MultiBootCheckComplete(mp) == 0) {
      MultiBootWaitSendDone();
      goto output_burst;
    }

    /* Timeout check */
    if (MultiBootCheckComplete(mp) == 0) {
      if (mp -> handshake_timeout == 0) {
        MultiBootInit(mp);
        return MULTIBOOT_ERROR_HANDSHAKE_FAILURE;
      }
      mp -> handshake_timeout--;
    }

    return 0;
  }

  switch (mp -> probe_count) {
  case 0:
    /* client not doing recognition
     * Value should be CLIENT_INFO 000 0 ccc 0
     * First, check if some kind of response(other than 0xffff) 
     * from machine.
     */
    k = 0x0e;
    for (i = MULTIBOOT_NCHILD; i != 0; i--) {
      if (*(vu16 *)(REG_SIOMULTI0 + i * 2) != 0xffff) {
        break;
      }
      k >>= 1;
    }

    k &= 0x0e; /* 4P-2P: d3-d1 is 1 */
    mp -> response_bit = k;

    /* Machine recognized as client, 
     * must be CLIENT_INFO 000 0 ccc 0.
     */
    for (i = MULTIBOOT_NCHILD; i != 0; i--) {
      j = *(vu16 *)(REG_SIOMULTI0 + i * 2);
      if (mp -> client_bit & (1 << i)) {
        if (j != ((MULTIBOOT_CLIENT_INFO << 8)
                  | (1 << i))) {
          /* Need to do recognition processing again */
          k = 0;
          break;
        }
      }
    }

    mp -> client_bit &= k;

    if (k == 0) {
      /* From client, until at least one returns value other than 
       * 0xffff, maintain fixed time until redo of recognition processing
       */
      mp -> check_wait = MULTIBOOT_CONNECTION_CHECK_WAIT;
    }
    
    if (mp -> check_wait) {
      mp -> check_wait--;
    } else {
      /* If machine with response and client status do not match up,
       * do recognition processing again.
       */
      if (mp -> response_bit != mp -> client_bit) {
        MultiBootStartProbe(mp);
        goto case_1;
      }
    }
    
output_master_info:
    /* Output MASTER_INFO 000 0 ccc 0.
     */
    return MultiBootSend(mp,
                        (MULTIBOOT_MASTER_INFO << 8)
                        | mp -> client_bit);
case_1:
  case 1:
    /* Start recognition.
     * Those where in data is CLIENT_INFO 000 0 ccc 0
     * are recognized.
     */
    mp -> probe_target_bit = 0;
    for (i = MULTIBOOT_NCHILD; i != 0; i--) {
      j = *(vu16 *)(REG_SIOMULTI0 + i * 2);
      if ((j >> 8) == MULTIBOOT_CLIENT_INFO) {
        /* Check validity of 000 0 ccc 0.
         * If 4P, 0x08
         * If 3P, 0x04
         * If 2P, 0x02
         * If not so, invalid.
         */
        MultiBoot_required_data[i - 1] = j; /* During processing next time
        must be same value */
        j &= 0xff;
        if (j == (1 << i)) {
          mp -> probe_target_bit |= j;
        }
      }
    }

    if (mp -> response_bit != mp -> probe_target_bit) {
      goto output_master_info;
    }

    /* Send MASTER_START_PROBE 000 0 ccc 0.
     * Here the recognized bits are 000, ccc.
     * If not possible that 000 or ccc, not recognized by master.
     */
    
    mp -> probe_count = 2;
    return MultiBootSend(mp,
                         (MULTIBOOT_MASTER_START_PROBE << 8)
                        | mp -> probe_target_bit);

  case 2:
    /* Must be CLIENT_INFO 000 0 ccc 0.
     * Output header +0,+1 bytes.
     */
    for (i = MULTIBOOT_NCHILD; i != 0; i--) {
      if (mp -> probe_target_bit & (1 << i)) {
        j = *(vu16 *)(REG_SIOMULTI0 + i * 2);
        if (j != MultiBoot_required_data[i - 1]) {
          /* Problem with client recognition */
          mp -> probe_target_bit ^= 1 << i;
        }
      }
    }
    goto output_header;

  case 0xd0:
    /* Request for start of master server.
     * If CLIENT_INFO 000 0 ccc 0, not ready
     * to download client.
     * If all clients CLIENT_DLREADY, call system
     * call and transfer boot program.
     */
    k = 1; /* Okay to go to download start? */
    for (i = MULTIBOOT_NCHILD; i != 0; i--) {
      j = *(vu16 *)(REG_SIOMULTI0 + i * 2);
      /* handshake data from client
       * Regardless of recognition, save for all machines
       */
      mp -> client_data[i - 1] = j;
      if (mp -> probe_target_bit & (1 << i)) {
        if ((j >> 8) != MULTIBOOT_CLIENT_INFO
            && (j >> 8) != MULTIBOOT_CLIENT_DLREADY) {
          MultiBootInit(mp);
          return MULTIBOOT_ERROR_NO_DLREADY; /* No response saying ready to do download */
        }
        if (j == MultiBoot_required_data[i - 1]) {
          /* CLIENT_INFO 000 0 ccc 0
           * Was at least one machine not ready for download,
           * so not start download yet.
           */
          k = 0;
        }
      }
    }

    if (k == 0) {
      /* Not start download yet.
       * Send request for download preparation
       */
      return MultiBootSend(mp,
                           (MULTIBOOT_MASTER_REQUEST_DLREADY << 8)
                           | mp -> palette_data);
    }

    /* All machines ready to download */
    mp -> probe_count = 0xd1;

    k = 0x11;
    for (i = MULTIBOOT_NCHILD; i != 0; i--) {
      /* handshake data */
      k += mp -> client_data[i - 1];
    }
    mp -> handshake_data = k;
    return MultiBootSend(mp,
                         (MULTIBOOT_MASTER_START_DL << 8) | (k & 0xff));

  case 0xd1:
    /* Send MASTER_START_DL
     * Should be CLIENT_DLREADY.
     */
    for (i = MULTIBOOT_NCHILD; i != 0; i--) {
      j = *(vu16 *)(REG_SIOMULTI0 + i * 2);
      if (mp -> probe_target_bit & (1 << i)) {
        if ((j >> 8) != MULTIBOOT_CLIENT_DLREADY) {
          MultiBootInit(mp);
          return MULTIBOOT_ERROR_NO_DLREADY; /* No response saying ready to do download */
        }
      }
    }

    i = MultiBoot(mp);

    if (i == 0) {
      /* complete
       * Make mp -> probe_count into 0xe0(request for handshake start).
       */
      mp -> probe_count = 0xe0;
      mp -> handshake_timeout = MULTIBOOT_HANDSHAKE_TIMEOUT;
      return 0;
    }
    MultiBootInit(mp);
    /* With system call failure, possible that client still in
     * receive data status.
     * Therefore, until retry, do not send anything including MASTER_INFO,
     * during time for "Client has timeout error".
     */
    mp -> check_wait = MULTIBOOT_CONNECTION_CHECK_WAIT * 2;
    return MULTIBOOT_ERROR_BOOT_FAILURE; /* Transfer failed */

  default:
    /* 4-...: Doing recognition processing
     * 4 -> +0,1 byte
     * 6 -> +2,3
     * 8 -> +4,5
     *  :
     * 0xc2 -> +0xbe, 0xbf byte
     * Output header data.
     */

    /* When client is being recognized,
     * value is MASTER_START_PROBE - 1, ..-2, ..., 0
     * 0x61 - 1,               0x5f
     * lower bytes are 000 0 ccc 0 
     */
    for (i = MULTIBOOT_NCHILD; i != 0; i--) {
      if (mp -> probe_target_bit & (1 << i)) {
        j = *(vu16 *)(REG_SIOMULTI0 + i * 2);
        if ((j >> 8) != (MULTIBOOT_MASTER_START_PROBE + 1 - (mp -> probe_count >> 1))
            || ((j & 0xff) != (1 << i))) {
          /* Problem with client recognition */
          mp -> probe_target_bit ^= 1 << i;
        }
      }
    }

    if (mp -> probe_count == 0xc4) {
      /* From recognized, those leftover last are 
       * qualified as client.
       */
      mp -> client_bit = mp -> probe_target_bit & 0x0e;
      mp -> probe_count = 0;
      goto output_master_info;
    }

output_header:
    /* If no target, ends with error in middle. */
    if (mp -> probe_target_bit == 0) {
      MultiBootInit(mp);
      return MULTIBOOT_ERROR_NO_PROBE_TARGET; /* No recognized target */
    }
    
    mp -> probe_count += 2;
    if (mp -> probe_count == 0xc4) {
      /* When getting final data, send MASTER_INFO 000 0 ccc 0.
       * If do not problem with timing.
       */
      goto output_master_info;
    }
    i = MultiBootSend(mp,
                      (mp -> masterp[mp -> probe_count - 4 + 1] << 8)
                      | mp -> masterp[mp -> probe_count - 4]);

    if (i) {
      return i;
    }
    /* If Low speed recognition mode, for each frame of call, 2 bytes of 
     * communication.
     * If High speed recognition mode,
     * (time to end of communication + sufficient time for slave 
     * interrupt processing)
     * Wait, continued communication.
     */
    if (mp -> server_type == MULTIBOOT_SERVER_TYPE_QUICK) {
      MultiBootWaitSendDone();
      goto output_burst;
    }
    return 0;
  }
  /* never comes here */
}


/*------------------------------------------------------------------*/
/*                       Send Data                                  */
/*------------------------------------------------------------------*/

/*
 * If connection has problem, non-0
 */
static int MultiBootSend(MultiBootParam *mp, u16 data)
{
  int i;

  /* If SC7 is on, problem has occurred.
   * There may be a problem with the connection(connected to JOY) and      
   * communication does not have timeout error.
   * (reconnect cable) May be first communication so no check for
   * SC6, connection ID.
   */
  i = *(vu16 *)REG_SIOCNT & (SIO_MULTI_BUSY | SIO_MULTI_SD | SIO_MULTI_SI);
  if (i != SIO_MULTI_SD) {
    MultiBootInit(mp);
    return i ^ SIO_MULTI_SD;
  }
  *(vu16 *)REG_SIODATA8 = data;
  *(vu16 *)REG_SIOCNT = SIO_MULTI_MODE | SIO_START | SIO_115200_BPS;
  

  mp -> sendflag = 1;
  return 0;
}


/*------------------------------------------------------------------*/
/*                    Start recognition of client                   */
/*------------------------------------------------------------------*/

void MultiBootStartProbe(MultiBootParam *mp)
{
  if (mp -> probe_count != 0) {
    MultiBootInit(mp);
    return;
  }
  mp -> check_wait = 0;
  mp -> client_bit = 0;
  mp -> probe_count = 1;
}


/*------------------------------------------------------------------*/
/*                   Start send from master server                  */
/*------------------------------------------------------------------*/

void MultiBootStartMaster(MultiBootParam *mp, u8 *srcp, int length, u8 palette_color, s8 palette_speed)
{
  int i;
  if (mp -> probe_count != 0
      || mp -> client_bit == 0
      || mp -> check_wait != 0
      ) {
    /* Recognition processing, cannot do processing */
    MultiBootInit(mp);
    return;
  }
  mp -> boot_srcp = srcp;
  length = (length + 15) & ~15; /* 16 byte units */
  if (length < MULTIBOOT_SEND_SIZE_MIN || length > MULTIBOOT_SEND_SIZE_MAX) {
    /* More than number or transfer bytes */
    MultiBootInit(mp);
    return;
  }
  mp -> boot_endp = srcp + length;
  switch (palette_speed) {
  case -4:
  case -3:
  case -2:
  case -1:
    i = (palette_color << 3) | (3 - palette_speed);
    break;
  case 0:
    i = 0x38 | palette_color;
    break;
  case 1:
  case 2:
  case 3:
  case 4:
    i = (palette_color << 3) | (palette_speed - 1);
    break;
  }
  mp -> palette_data = ((i & 0x3f) << 1) | 0x81;
  mp -> probe_count = 0xd0;
}


/*------------------------------------------------------------------*/
/*                       Check transfer completion                  */
/*------------------------------------------------------------------*/

int MultiBootCheckComplete(MultiBootParam *mp)
{
  if (mp -> probe_count == 0xe9) {
    /* Transfer complete status */
    return 1;
  }
  /* middle of recognition, haven't started transfer, or transfer failure */
  return 0;
}

/*------------------------------------------------------------------*/
/*                  Handshake (final confirmation of boot)          */
/*------------------------------------------------------------------*/

static int MultiBootHandShake(MultiBootParam *mp)
{
  int i, j;
#define send_data (mp -> system_work[0])
#define must_data (mp -> system_work[1])

  switch (mp -> probe_count) {
case_0xe0:
  case 0xe0:
    /* Master sends 0x0000. */
    mp -> probe_count = 0xe1;
    must_data = 0x0000;
    send_data = 0x100000; /* Right before next send >>5  */
    return MultiBootSend(mp, 0x0000);

case_e1:
  default: /* 0xe1-0xe6
            * If expected data does not come, do again from case_0xe0.
            * 0xe1: After master sends 0x0000. All slaves must be 0x0000.
            * 0xe2: After master sends 0x8000.All slaves must be 0x0000.
            * 0xe3:        0x0400                            0x8000
            * 0xe4:        0x0020                            0x0400
            * 0xe5:        0x0001                            0x0020
            * 0xe6:        0x0000                            0x0001
            */

    for (i = MULTIBOOT_NCHILD; i != 0; i--) {
      j = *(vu16 *)(REG_SIOMULTI0 + i * 2);
      if ((mp -> client_bit & (1 << i))
          && j != must_data) {
        /* Expected data still hasn't come from all slaves. */
        goto case_0xe0;
      }
    }
    mp -> probe_count++;
    must_data = send_data & 0xffff;
    if (send_data == 0x0000) {
      /* This time send initial code low.
       */
      must_data = mp -> masterp[0xac] | (mp -> masterp[0xad] << 8);
      send_data = must_data << 5; /* right before sending >>5 */
    }
    send_data >>= 5;
output_common:
    return MultiBootSend(mp, send_data);

  case 0xe7: /* Master sent initial code, low. All slaves must be same. */
  case 0xe8: /* Master sent initial code, high. All slaves must be same. */
    for (i = MULTIBOOT_NCHILD; i != 0; i--) {
      j = *(vu16 *)(REG_SIOMULTI0 + i * 2);
      if ((mp -> client_bit & (1 << i))
          && j != must_data) {
        /* Desired data did not come from all slaves.
         * If reach this point and have error, stop(infinite loop) slave, 
         * and no retry by master.
         * On master's screen display,
         * "Communicsation failure. Turn off power and check connection. 
         *  Turn on power again."
         */
        MultiBootInit(mp);
        return MULTIBOOT_ERROR_HANDSHAKE_FAILURE;
      }
    }

    mp -> probe_count++;
    if (mp -> probe_count == 0xe9) {
      /* Handshake Success! */
      return 0;
    }

    /* This time send the initial code, high.
     */
    send_data = mp -> masterp[0xae] | (mp -> masterp[0xaf] << 8);
    must_data = send_data;
    goto output_common;
  }
#undef send_data
#undef must_data
}

/*------------------------------------------------------------------*/
/*                     Wait Cycle                                   */
/*------------------------------------------------------------------*/

static void MultiBootWaitCycles(u32 cycles)
{
  /* Depending on if this is in CPU internal working, CPU external 
   * working, ROM, the CPU cycles used for one of this function's wait 
   * loops is different.
   * CPU External Working (0x02XXXXXX) ... 12 cycles/loop
   * ROM        (0x08XXXXXX) ... 13 cycles/loop 
   *            (Have prefetch  Setup maximum speed)
   * CPU Internal Working (0x03XXXXXX) ... 4  cycles/loop
   * If address area other than above, temporarily use 4 cycles/loop.
   * If set up lower cycles/loop than actual, 
   * can get specified cycle number wait.
   *
   * Use AGB system clock 16.78MHz as hint for argument, cycles.
   * If use 0x1000000 (16777216) with cycles approximately 1 second wait.
   * (If V blank interrupt is processed during this, actual wait is longer)
   */

#ifdef  __GNUC__
  /* GCC code16 */
  asm("mov r2, pc");
  asm("lsr r2, #24");
  asm("mov r1, #12");
  asm("cmp r2, #0x02");
  asm("beq MultiBootWaitCyclesLoop");

  asm("mov r1, #13");
  asm("cmp r2, #0x08");
  asm("beq MultiBootWaitCyclesLoop");

  asm("mov r1, #4");

  asm("MultiBootWaitCyclesLoop:");
  asm("sub r0, r1");
  asm("bgt MultiBootWaitCyclesLoop");
#else
  /* TCC */
  __asm{mov r2, pc};
  __asm{lsr r2, #24};
  __asm{mov r1, #12};
  __asm{cmp r2, #0x02};
  __asm{beq MultiBootWaitCyclesLoop};

  __asm{mov r1, #13};
  __asm{cmp r2, #0x08};
  __asm{beq MultiBootWaitCyclesLoop};

  __asm{mov r1, #4};

  __asm{MultiBootWaitCyclesLoop:};
  __asm{sub r0, r1};
  __asm{bgt MultiBootWaitCyclesLoop};
#endif
}

/*------------------------------------------------------------------*/
/*             Check if communication completed within fixed time   */
/*------------------------------------------------------------------*/

static void MultiBootWaitSendDone(void)
{
  int i;

  /* If cannot detect communication end within fixed time(1 frame),
   * remove loop.
   * Even if fast this loop takes 9 cycles/loop,
   * (ldr=3, and=1, branch skip=1, i++=1, branch=3)
   * try loop for 1 frame.
   * Number of times do loop is,
   * 0x1000000 (16.78MHz=cycles/sec) / 60 (frames/sec) / 9 (cycles/loop)
   * = approximately 31069 (loops/frame)
   */
  for (i = 0; i < 31069; i++) {
    if ((*(vu16 *)REG_SIOCNT & SIO_START) == 0) {
      break;
    }
  }

  /* Sufficient time for slave's interrupt processing */
  MultiBootWaitCycles(600);
}
